// linalg.h
#ifndef LINALG_H
#define LINALG_H

#include "xil_types.h"
#include <math.h> // For sinf, cosf, tanf, sqrtf, fabsf, powf

// --- Vector3 ---
typedef struct {
    float x, y, z;
} Vector3;

// --- Vector4 ---
typedef struct {
    float x, y, z, w;
} Vector4_f; // Using _f suffix to avoid conflict with potentially other Vec4 definitions

// --- Matrix4x4 ---
typedef struct {
    float m[4][4];
} Matrix4;

// --- 向量操作 ---
static inline Vector3 Vec3_add(Vector3 a, Vector3 b) {
    return (Vector3){a.x + b.x, a.y + b.y, a.z + b.z};
}

static inline Vector3 Vec3_sub(Vector3 a, Vector3 b) {
    return (Vector3){a.x - b.x, a.y - b.y, a.z - b.z};
}

// 向量与标量乘法
static inline Vector3 Vec3_mul_scalar(Vector3 v, float s) {
    return (Vector3){v.x * s, v.y * s, v.z * s};
}

// 向量元素级乘法 (用于颜色相乘)
static inline Vector3 Vec3_multiply_elements(Vector3 a, Vector3 b) {
    return (Vector3){a.x * b.x, a.y * b.y, a.z * b.z};
}

static inline float Vec3_dot(Vector3 a, Vector3 b) {
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

static inline float Vec3_length(Vector3 v) {
    return sqrtf(Vec3_dot(v, v));
}

static inline Vector3 Vec3_normalize(Vector3 v) {
    float len = Vec3_length(v);
    if (len > 1e-6f) { // Avoid division by zero
        return Vec3_mul_scalar(v, 1.0f / len);
    }
    return (Vector3){0.0f, 0.0f, 0.0f};
}

static inline Vector3 Vec3_cross(Vector3 a, Vector3 b) {
    return (Vector3){
        a.y * b.z - a.z * b.y,
        a.z * b.x - a.x * b.z,
        a.x * b.y - a.y * b.x
    };
}

// 反转向量方向 (新增)
static inline Vector3 Vec3_negate(Vector3 v) {
    return (Vector3){-v.x, -v.y, -v.z};
}

// --- 矩阵操作 ---

// 创建单位矩阵
static inline Matrix4 Mat4_identity() {
    Matrix4 mat = {0};
    mat.m[0][0] = 1.0f;
    mat.m[1][1] = 1.0f;
    mat.m[2][2] = 1.0f;
    mat.m[3][3] = 1.0f;
    return mat;
}

// 矩阵乘法 C = A * B
static inline Matrix4 Mat4_multiply(Matrix4 A, Matrix4 B) {
    Matrix4 C;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            C.m[i][j] = 0.0f;
            for (int k = 0; k < 4; ++k) {
                C.m[i][j] += A.m[i][k] * B.m[k][j];
            }
        }
    }
    return C;
}

// 矩阵-Vector4 乘法 (新增)
static inline Vector4_f Mat4_mul_Vec4(Matrix4 M, Vector4_f V) {
    Vector4_f result;
    result.x = M.m[0][0] * V.x + M.m[0][1] * V.y + M.m[0][2] * V.z + M.m[0][3] * V.w;
    result.y = M.m[1][0] * V.x + M.m[1][1] * V.y + M.m[1][2] * V.z + M.m[1][3] * V.w;
    result.z = M.m[2][0] * V.x + M.m[2][1] * V.y + M.m[2][2] * V.z + M.m[2][3] * V.w;
    result.w = M.m[3][0] * V.x + M.m[3][1] * V.y + M.m[3][2] * V.z + M.m[3][3] * V.w;
    return result;
}

// 矩阵-Vector3 乘法 (不进行透视除法，用于变换位置)
// V.w 假定为 1.0f
static inline Vector3 Mat4_mul_Vec3_Position(Matrix4 M, Vector3 V) {
    Vector3 result;
    result.x = M.m[0][0] * V.x + M.m[0][1] * V.y + M.m[0][2] * V.z + M.m[0][3];
    result.y = M.m[1][0] * V.x + M.m[1][1] * V.y + M.m[1][2] * V.z + M.m[1][3];
    result.z = M.m[2][0] * V.x + M.m[2][1] * V.y + M.m[2][2] * V.z + M.m[2][3];
    return result;
}

// 矩阵-Vector3 乘法 (不进行透视除法和不使用平移分量，用于变换方向向量/法线)
// V.w 假定为 0.0f
static inline Vector3 Mat4_mul_Vec3_NoDiv_Direction(Matrix4 M, Vector3 V) {
    Vector3 result;
    result.x = M.m[0][0] * V.x + M.m[0][1] * V.y + M.m[0][2] * V.z;
    result.y = M.m[1][0] * V.x + M.m[1][1] * V.y + M.m[1][2] * V.z;
    result.z = M.m[2][0] * V.x + M.m[2][1] * V.y + M.m[2][2] * V.z;
    return result;
}

// 创建平移矩阵
static inline Matrix4 Mat4_translate(float x, float y, float z) {
    Matrix4 mat = Mat4_identity();
    mat.m[0][3] = x;
    mat.m[1][3] = y;
    mat.m[2][3] = z;
    return mat;
}

// 创建绕 Y 轴旋转矩阵 (弧度)
static inline Matrix4 Mat4_rotateY(float angle_rad) {
    Matrix4 mat = Mat4_identity();
    float c = cosf(angle_rad);
    float s = sinf(angle_rad);
    mat.m[0][0] = c;
    mat.m[0][2] = s;
    mat.m[2][0] = -s;
    mat.m[2][2] = c;
    return mat;
}

// 创建缩放矩阵
static inline Matrix4 Mat4_scale(float x, float y, float z) {
    Matrix4 mat = Mat4_identity();
    mat.m[0][0] = x;
    mat.m[1][1] = y;
    mat.m[2][2] = z;
    return mat;
}

// 创建透视投影矩阵 (右手坐标系，Z值映射到[-1,1])
// 此矩阵与 OpenGL 类似，Z轴通常映射到NDC的[-1, 1]
static inline Matrix4 Mat4_perspective(float fov_y_rad, float aspect, float near_plane, float far_plane) {
    Matrix4 mat = {0};
    float tan_half_fov = tanf(fov_y_rad / 2.0f);
    float f = 1.0f / tan_half_fov;

    mat.m[0][0] = f / aspect;
    mat.m[1][1] = f;
    mat.m[2][2] = (far_plane + near_plane) / (near_plane - far_plane); // (f+n)/(n-f)
    mat.m[2][3] = (2.0f * far_plane * near_plane) / (near_plane - far_plane); // (2fn)/(n-f)
    mat.m[3][2] = -1.0f; // for W component
    return mat;
}

// 创建 LookAt 视图矩阵 (右手坐标系)
static inline Matrix4 Mat4_lookAt(Vector3 eye, Vector3 center, Vector3 up) {
    // 摄像机看向负Z轴方向，所以Z轴是 eye - center
    Vector3 z_axis = Vec3_normalize(Vec3_sub(eye, center));
    
    // 摄像机右侧X轴是 up 叉乘 z_axis
    Vector3 x_axis = Vec3_normalize(Vec3_cross(up, z_axis));
    
    // 摄像机上方向Y轴是 z_axis 叉乘 x_axis
    Vector3 y_axis = Vec3_cross(z_axis, x_axis); // 无需再次归一化，因为z_axis和x_axis是正交单位向量

    Matrix4 mat = Mat4_identity();
    // 视图矩阵的旋转部分 (3x3 左上角)
    // 第一行是相机局部X轴（右侧）
    mat.m[0][0] = x_axis.x; mat.m[0][1] = x_axis.y; mat.m[0][2] = x_axis.z;
    // 第二行是相机局部Y轴（上方）
    mat.m[1][0] = y_axis.x; mat.m[1][1] = y_axis.y; mat.m[1][2] = y_axis.z;
    // 第三行是相机局部Z轴（前向，即看向负z_axis方向）
    mat.m[2][0] = z_axis.x; mat.m[2][1] = z_axis.y; mat.m[2][2] = z_axis.z; 

    // 视图矩阵的平移部分
    mat.m[0][3] = -Vec3_dot(x_axis, eye);
    mat.m[1][3] = -Vec3_dot(y_axis, eye);
    mat.m[2][3] = -Vec3_dot(z_axis, eye);
    return mat;
}


#endif // LINALG_H
